; last changed 15 june, after watching in speccy's ELITE

edin equ 64
diver:
;DE = DE/BC c округлением ответа
 sla  e
 rl   d
 ld   a,b
 cpl
 ld   b,a
 ld   a,c
 cpl
 ld   c,a
 inc  bc
 ld   hl,0

 rept 16
 ld   a,l
 adc  a,a
 ld   l,a
 ld   a,h
 adc  a,a
 ld   h,a
 add  hl,bc
 jr   c,@+7
 ld   h,a
 ld   a,l
 sub  c
 ld   l,a
 or   a

 rl   e
 rl   d
 endr
 sra  b
 rr   c
 add  hl,bc
;
 ret  nc
 inc  de
 ret

;------------------
Multiple: MACRO         
 ld   h,3+sqrTab/256  ; быстрое умножение в пределах [-127, 127]
 ld   d,a               ; вход :  n1,n2 at d and e
 bit  7,d               ; выход:  hl=n1*n2
 jr   nz,.lp1\@         ; используется таблица с квадратами чисел
                        ;   и таблица с минус квадратами    
                        ; table: adress     low
                        ;        adress+256 high
                        ; used 4ab=(a+b)(a+b)-(a-b)(a-b)
                        ; d>0
 bit  7,e
 ld   a,e
 jr   z,.lp2\@
 cpl                    ; e=-e
 inc  a
 ld   e,a               ;

 ld   a,d
.lp3\@: 
 sub  e
 jr   nc,@+4
 cpl
 inc  a
 ld   l,a               ; |a-b|
 ld   a,d
 add  a,e               ;  a+b
 ld   d,[hl]            ; 
 dec  h                 ; de=-(a-b)(a-b)
 ld   e,[hl]            ;
 dec  h
 ld   l,a
 ld   a,[hl]            ;
 dec  h                 ; hl= (a+b)(a+b)
 ld   l,[hl]            ;
 ld   h,a
 add  hl,de             ; hl= 4ab
 ld   a,h               ;
 cpl                    ; a=-a
 inc  a
 jr   .lp4\@

.lp1\@:
 ld   a,d
 cpl                    ; 
 inc  a
 ld   d,a               ; d=-d
 bit  7,e
 jr   z,.lp3\@          ; знаки разные => результат будет <0
 ld   a,e
 cpl  
 inc  a
 ld   e,a               ; both numbers was less zero=> Result will >0
.lp2\@:
 sub  d
 jr   nc,@+4
 cpl
 inc  a
 ld   l,a               ; |a-b|
 ld   a,d
 add  a,e               ;  a+b
 ld   d,[hl]            ; 
 dec  h                 ; de=-(a-b)(a-b)
 ld   e,[hl]            ;
 dec  h
 ld   l,a
 ld   a,[hl]            ;
 dec  h                 ; hl= (a+b)(a+b)
 ld   l,[hl]            ;
 ld   h,a
 add  hl,de             ; hl= 4ab
 ld   a,h               ; a=hl/256
.lp4\@
 ENDM

;-------------------------------------------
RotateAroundXYZ:

 ld   a,[angleX]        ; step 0:
 ld   h,sintab/256      ; left (work) Matrix will [Mx]
 ld   l,a
 ld   a,[hl]
 ldio [wp+5],a          ; sin a
 cpl  
 inc  a
 ldio [wp+7],a          ;-sin a
 ld   a,l
 add  a,$40
 ld   l,a
 ld   a,[hl]
 ldio [wp+4],a          ; cos a
 ldio [wp+8],a          ;
 xor  a                 
 ldio [wp+1],a          ; 0 elements
 ldio [wp+2],a
 ldio [wp+3],a
 ldio [wp+6],a
 ldio [vp+1],a          ; 0
 ld   a,edin            ; 1 element
 ldio [wp+0],a

 ld   a,[angleY]        ; step 1.1:
 ld   l,a               ; first column of [My] Matrix will work vector
 ld   c,[hl]            ; sin b
 ld   a,l
 add  a,$40
 ld   l,a
 ld   b,[hl]            ; cos b
 push bc
 ld   a,b
 ldio [vp+0],a          ; cos b
 ld   a,c
 ldio [vp+2],a          ; sin b
 call MatrixVectorMulti

 ldio a,[nvp+0]         ; new vector = [Mx] * vy1 
 ldio [tp+0],a          ;
 ldio a,[nvp+1]
 ldio [tp+3],a
 ldio a,[nvp+2]
 ldio [tp+6],a 

 xor  a                 ; step 1.2:
 ldio [vp+0],a          ; second column of [My] will work vector
 ldio [vp+2],a          ; it: 0
 ld   a,edin            ;     1
 ldio [vp+1],a          ;     0
 call MatrixVectorMulti

 ldio a,[nvp+0]         ; new vector = [Mx] * vy2 
 ldio [tp+1],a          ;
 ldio a,[nvp+1]
 ldio [tp+4],a
 ldio a,[nvp+2]
 ldio [tp+7],a 

 pop  bc                ; c=sin b,  b=cos b
 ld   a,c               ; step 1.3:
 cpl                    ; third column of [My] will work vector
 inc  a
 ldio [vp+0],a          ;-sin b
 xor  a
 ldio [vp+1],a          ; 0
 ld   a,b   
 ldio [vp+2],a          ; cos b
 call MatrixVectorMulti ; after this calling, new vector = [Mx] * vy3 

;                       ; step 2:
                        ; transfer [Mx]*[My] to left (work) Matrix 
 ldio a,[nvp+0]         ; 
 ldio [wp+2],a          ; 
 ldio a,[nvp+1]         ;
 ldio [wp+5],a          ; transfer third column 
 ldio a,[nvp+2]         ;
 ldio [wp+8],a          ;

 ldio a,[tp+0]          ; transfer first column
 ldio [wp+0],a          ;
 ldio a,[tp+3]          ;
 ldio [wp+3],a          ;
 ldio a,[tp+6]          ;
 ldio [wp+6],a          ;

 ldio a,[tp+1]          ; transfer second column
 ldio [wp+1],a          ;
 ldio a,[tp+4]          ;
 ldio [wp+4],a          ;
 ldio a,[tp+7]          ;
 ldio [wp+7],a          ;
;
 ld   a,[angleZ]
 ld   h,sintab/256      ; step 3.1:
 ld   l,a               ; first column of [Mz] will work vector
 ld   c,[hl]            ; sin g
 ld   a,l
 add  a,$40
 ld   l,a
 ld   b,[hl]            ; cos g
 push bc
 ld   a,b 
 ldio [vp+0],a          ; cos g
 ld   a,c
 cpl 
 inc  a
 ldio [vp+1],a          ;-sin g
 xor  a
 ldio [vp+2],a          ; 0
 call MatrixVectorMulti

 ldio a,[nvp+0]         ; new vector = [Mx]*[My]* vz1 
 ldio [tp+0],a          ;
 ldio a,[nvp+1]
 ldio [tp+3],a
 ldio a,[nvp+2]
 ldio [tp+6],a 
 pop  bc
                        ; step 3.2:
 ld   a,c               ; second column of [Mz] will work vector
 ldio [vp+0],a          ; sin g
 ld   a,b
 ldio [vp+1],a          ; cos g
 xor  a
 ldio [vp+2],a          ; 0
 call MatrixVectorMulti

 ldio a,[nvp+0]         ; new vector = [Mx]*[My] * vz2 
 ldio [tp+1],a          ;
 ldio a,[nvp+1]
 ldio [tp+4],a
 ldio a,[nvp+2]
 ldio [tp+7],a 
                        ; step 3.3:
 xor  a                 ; third column of [Mz] will work vector
 ldio [vp+0],a
 ldio [vp+1],a          ; 0
 ld   a,edin            ; 0
 ldio [vp+2],a          ; 1
 call MatrixVectorMulti
                        ; step 4:
                        ; Transfer [Mx]*[My]*[Mz] to left (work) Matrix
 ldio a,[nvp+0]         ; 
 ldio [wp+2],a          ; 
 ldio a,[nvp+1]         ;
 ldio [wp+5],a          ; transfer third column 
 ldio a,[nvp+2]         ;
 ldio [wp+8],a          ;

 ldio a,[tp+0]          ; transfer first column
 ldio [wp+0],a          ;
 ldio a,[tp+3]          ;
 ldio [wp+3],a          ;
 ldio a,[tp+6]          ;
 ldio [wp+6],a          ;

 ldio a,[tp+1]          ; transfer second column
 ldio [wp+1],a          ;
 ldio a,[tp+4]          ;
 ldio [wp+4],a          ;
 ldio a,[tp+7]          ;
 ldio [wp+7],a          ;
 ret

;--------------------
MatrixVectorHLMulti:
 ld   a,[hli]           ; in : hl adress of vector
 ldio [vp+0],a
 ld   a,[hli]
 ldio [vp+1],a
 ld   a,[hli]
 ldio [vp+2],a
                        
MatrixVectorMulti:      ;      wp+0  m11 m12 m13   |
                        ;      wp+3  m21 m22 m23   | matrix
 ldio a,[wp+0]  ; m11   ;      wp+6  m31 m32 m33   |
 ld   e,a               ; use: vp+0   x   y   z     internal vector
 ldio a,[vp+0]  ; x     ;-------------------------------------

 Multiple               ; out: nvp    x'  y'  z'     new vector
 ld   b,a               ;
 ldio a,[wp+1]          ; m12   |---------------------------------
 ld   e,a               ;       | NB!
 ldio a,[vp+1]          ; y     | This routine not optimized for small size,
                        ;       | because speed optimization here is more
 Multiple               ;       | preferable.
 ld   c,a               ;       | Though,possible makes speed-up and for this.
 ldio a,[wp+2]          ; m13   | SORRY,this CPU have less registers then Z80!
 ld   e,a               ;       | 
 ldio a,[vp+2]          ; z     | Multiple is big macro (>64 bytes),but need 
                        ;       | to speed-up matrix-vector multiply. 
 Multiple               ;       | (without CALL/RET construction)
 add  a,b               ;        
 add  a,c               ;       
 ldio [nvp+0],a         ; x'
;--------------------------------------------
 ldio a,[wp+3]          ; m21     
 ld   e,a               
 ldio a,[vp+0]          ; x   
 Multiple               
 ld   b,a
 ldio a,[wp+4]          ; m22
 ld   e,a
 ldio a,[vp+1]          ; y
 Multiple
 ld   c,a
 ldio a,[wp+5]          ; m23
 ld   e,a
 ldio a,[vp+2]          ; z
 Multiple
 add  a,b
 add  a,c
 ldio [nvp+1],a         ; y'
;--------------------------------------------
 ldio a,[wp+6]          ; m31     
 ld   e,a               
 ldio a,[vp+0]          ; x   
 Multiple               
 ld   b,a
 ldio a,[wp+7]          ; m32
 ld   e,a
 ldio a,[vp+1]          ; y
 Multiple
 ld   c,a
 ldio a,[wp+8]          ; m33
 ld   e,a
 ldio a,[vp+2]          ; z
 Multiple
 add  a,b
 add  a,c
 ldio [nvp+2],a         ; z'
 ret
;---------------------------------------------